// Template for idle thread plus three schedulable threads

#include <NilRTOS.h>

// Use tiny unbuffered NilRTOS NilSerial library.
#include <NilSerial.h>
#include <Servo.h> 
#include <Wire.h>

#define MAP_MIN    0
#define MAP_MAX    1023
#define ACT_MIN    10
#define ACT_MAX    89

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;

volatile int servo_act = 0;

//------------------------------------------------------------------------------
// Declare a stack with 64 bytes beyond context switch and interrupt needs.
NIL_WORKING_AREA(waThread1, 64);

// Declare thread function for thread 1.
NIL_THREAD(Thread1, arg) {
  int pos = 0;
  while (TRUE) {

    if(!(servo_act & 0x0001)){
      nilThdSleepMilliseconds(1);
      continue;
    }
    
    // Sleep so lower priority threads can execute.
//    nilThdSleep(100);
    for(pos = MAP_MIN; pos < MAP_MAX; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      myservo1.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    for(pos = MAP_MAX; pos>=2; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      myservo1.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    // Dummy use of CPU - use nilThdSleep in normal app.
//    nilThdDelay(100);
  }
}
//------------------------------------------------------------------------------
// Declare a stack with 64 bytes beyond context switch and interrupt needs.
NIL_WORKING_AREA(waThread2, 64);

// Declare thread function for thread 2.
NIL_THREAD(Thread2, arg) {
int pos = 0;
  while (TRUE) {
    if(!(servo_act & 0x0002)){
      nilThdSleepMilliseconds(1);
      continue;
    }
    // Sleep so lower priority threads can execute.
//    nilThdSleep(100);
    for(pos = MAP_MIN; pos < MAP_MAX; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      myservo2.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    for(pos = MAP_MAX; pos>=2; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      myservo2.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    // Dummy use of CPU - use nilThdSleep in normal app.
//    nilThdDelay(100);
  }
}
//------------------------------------------------------------------------------
// Declare a stack with 64 bytes beyond context switch and interrupt needs.
NIL_WORKING_AREA(waThread3, 64);

// Declare thread function for thread 3.
NIL_THREAD(Thread3, arg) {
int pos = 0;
  while (TRUE) {
    if(!(servo_act & 0x0004)){
      nilThdSleepMilliseconds(1);
      continue;
    }
    // Sleep so lower priority threads can execute.
//    nilThdSleep(100);
    for(pos = MAP_MIN; pos < MAP_MAX; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      myservo3.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    for(pos = MAP_MAX; pos>=2; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      myservo3.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    // Dummy use of CPU - use nilThdSleep in normal app.
//    nilThdDelay(100);
  }
}
//------------------------------------------------------------------------------
// Declare a stack with 64 bytes beyond context switch and interrupt needs.
NIL_WORKING_AREA(waThread4, 64);

// Declare thread function for thread 3.
NIL_THREAD(Thread4, arg) {
int pos = 0;
  while (TRUE) {
    if(!(servo_act & 0x0008)){
      nilThdSleepMilliseconds(1);
      continue;
    }
    // Sleep so lower priority threads can execute.
//    nilThdSleep(100);
    for(pos = MAP_MIN; pos < MAP_MAX; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      myservo4.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    for(pos = 1023; pos>=2; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      myservo4.write(map(pos, MAP_MIN, MAP_MAX, ACT_MIN, ACT_MAX));              // tell servo to go to position in variable 'pos' 
      nilThdSleepMilliseconds(1);
    } 
    // Dummy use of CPU - use nilThdSleep in normal app.
//    nilThdDelay(100);
  }
}
//------------------------------------------------------------------------------
/*
 * Threads static table, one entry per thread.  A thread's priority is
 * determined by its position in the table with highest priority first.
 *
 * These threads start with a null argument.  A thread's name is also
 * null to save RAM since the name is currently not used.
 */
NIL_THREADS_TABLE_BEGIN()
NIL_THREADS_TABLE_ENTRY(NULL, Thread1, NULL, waThread1, sizeof(waThread1))
NIL_THREADS_TABLE_ENTRY(NULL, Thread2, NULL, waThread2, sizeof(waThread2))
NIL_THREADS_TABLE_ENTRY(NULL, Thread3, NULL, waThread3, sizeof(waThread3))
NIL_THREADS_TABLE_ENTRY(NULL, Thread4, NULL, waThread4, sizeof(waThread4))
NIL_THREADS_TABLE_END()
//------------------------------------------------------------------------------
void receiveEvent(int howMany)
{
  nilSysLock();
  servo_act = Wire.read();    // receive byte as an integer
  nilSysUnlock();
}

void setup() {
  Wire.begin(4);                // join i2c bus with address #4
  Wire.onReceive(receiveEvent); // register event
  myservo1.attach(6);
  myservo2.attach(5);
  myservo3.attach(9);
  myservo4.attach(3);
  // start kernel
  nilSysBegin();
}
//------------------------------------------------------------------------------
// Loop is the idle thread.  The idle thread must not invoke any
// kernel primitive able to change its state to not runnable.
void loop() {

}
